#include <ros/ros.h>
#include <controller_manager/controller_manager.h>
#include "gpt_robot_description/diffbot_hardware_interface.h"

int main(int argc, char** argv)
{
    ros::init(argc, argv, "diffbot_control_node");
    ros::NodeHandle nh;

    DiffBotHWInterface diffbot_hw(nh);
    diffbot_hw.init();

    controller_manager::ControllerManager cm(&diffbot_hw, nh);

    ros::Rate rate(50);
    while (ros::ok())
    {
        diffbot_hw.read(ros::Duration(rate.expectedCycleTime().toSec()));
        cm.update(ros::Time::now(), ros::Duration(rate.expectedCycleTime().toSec()));
        diffbot_hw.write(ros::Duration(rate.expectedCycleTime().toSec()));
        rate.sleep();
    }

    return 0;
}
